/*
 * Copyright (c) 2006-2021, Halo Team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-16     Eywen       the first version
 */
#ifndef MOTION_CONTROL_H_
#define MOTION_CONTROL_H_

#include "config_print.h"
#include "motion_step.h"
#include "extruder_step.h"
#include <rtthread.h>

#define ERROR_MOTION_COMPENSATION  true

#define MOTION_MAX_STOP_FREQ                (800)
#define MOTION_Z_MAX_FREQ                   (15000U)     /* 步进电机最大运行频率 */
#define MOTION_XY_MAX_FREQ                  (15000U)     /* 步进电机最大运行频率 */
#define MOTION_CONTROL_TASK_BUFF_LEN        (5U)         /* 预存5条路径 */
#define MOTION_CONTROL_GCODE_TASK_BUF_LEN   (5U)         /* 预存5条gcode运动任务 */

#define MOTION_XY_MIN_STOP_FREQ             (600)

typedef struct _velocity_vector{
    float v_x;
    float v_y;
    float v_z;
    float v;
}velocity_vector_t;

typedef struct _freq_vector{
    float f_x;
    float f_y;
    float f_z;
    float f;
}freq_vector_t;

typedef struct _location_vector{
    float pulse_x;
    float pulse_y;
    float pulse_z;
    float pulse_e;
}pulse_vector_t;


/* 速度时间计算关系：
 * v = a*t^2 + b*t + c */
typedef struct __velocity_time_eq{
    float a;
    float b;
    float c;
}velocity_time_equ_t;

/* 频率与时间计算关系：
 * f = a*t^2 + b*t + c
 * pulse = 1/3*a*t^3 + 1/2*b*t^2 + c*t + d*/
typedef struct __freq_time_para{
    float a;
    float b;
    float c;

    float freq_max;
    float task_schdule_time;
    float motion_pulse;     /* 总脉冲个数 */
    float time_inv;         /* 离散时间间隔 */
    float time_last;
    float freq_end;
    float freq_start;

    float feed_freq;    /* 挤出机的频率 */
    float feed_pulse;   /* 挤出机的脉冲个数 */

    pulse_vector_t distance;   /* 当前路径的偏差（起始点到终点的矢量，移动到原点为起始点） */

    float sin_alf_x;
    float sin_alf_y;

    int z_dir;
#if (ERROR_MOTION_COMPENSATION == true)
    float time_actual;
    float actual_pulse;
#endif

    motion_step_type_t step_type;
}freq_time_para_t;


/* gcode的单位，非控制的脉冲相关单位 */
typedef struct _motion{
    float speed;            /* 运动速度 */
#if IS_ENABLE(CONFIG_LASER)
    float laser_power;      /* 激光功率 */
#endif

#if IS_ENABLE(CONFIG_EXTRUDER)
    float feed_rate;        /* 挤出速率 */
#endif
    float x;                /* 目标x轴位置 */
    float y;                /* 目标y轴位置 */
    float z;                /* 目标z轴位置 */
    float e;                /* 目标e轴挤出量 */
}motion_t;

typedef struct _motion_control_ops motion_control_ops_t;

typedef struct _motion_control{
    freq_time_para_t para;

     /* 存放每个直线的任务频率和脉冲的计算参数, 存放freq_time_para_t计算任务 */
    ringbuff_t *handle_list;
    /* 存放gcode运动节点 gcode_parser_motion_t */
    ringbuff_t *gcode_motion_list;

    motion_step_t motion_step_x;
    motion_step_t motion_step_y;
    motion_step_t motion_step_y_1;
    motion_step_t motion_step_z;

    extruder_step_t extruder_step_e;

    motion_control_ops_t *ops;

    rt_thread_t task_add_thread_id;
    rt_thread_t task_pop_thread_id;

    pulse_vector_t end_location;          /* 结束位置 */

}motion_control_t;

#define MOTION_CONTROL_ADD_TASK_THREAD_PRIORITY   (10)
#define MOTION_CONTROL_ADD_TASK_THREAD_TIMESLICE  (10u)
#define MOTION_CONTROL_ADD_TASK_STACK_SIZE        (2048u)
#define MOTION_CONTROL_POP_TASK_THREAD_PRIORITY   (10)
#define MOTION_CONTROL_POP_TASK_THREAD_TIMESLICE  (10u)
#define MOTION_CONTROL_POP_TASK_STACK_SIZE        (2048u)

typedef struct _motion_control_ops{
    bool (*init)(motion_control_t *motion_control);
    //bool (*add_line_task)(motion_control_t *motion_control, freq_time_para_t *new_para);
    //bool (*add_vector)(motion_control_t *motion_control,
    //        pulse_vector_t *add_pulse_vector, float new_freq);
    bool (*task_full)(motion_control_t *motion_control);
    //bool (*pop_task)(motion_control_t *motion_control, freq_time_para_t *para);
    bool (*task_empty)(motion_control_t *motion_control);
    int (*task_count)(motion_control_t *motion_control);
    bool (*add_gcode_task)(motion_control_t *motion_control, motion_t *gocde_task);
    bool (*start)(motion_control_t *motion_control);
    bool (*step_init)(motion_control_t *motion_control);
    bool (*start_all_step)(motion_control_t *motion_control);
    bool (*gcode_task_empty)(motion_control_t *motion_control);
    //float (*speed_to_freq)(motion_control_t *motion_control);
}motion_control_ops_t;

void freq_time_para_set(freq_time_para_t *para, float f_start, float f_end,
                        float f_ave, uint32_t f_pulse, float time_inv,
                        float freq_max, motion_step_type_t step_type, int z_dir);
void motion_control_test(void);
bool motion_control_register(motion_control_t *device);
motion_control_t *motion_control_create(void);
//bool motion_control_line_task_to_xyze_axis(freq_time_para_t *freq_time_para,
//        smart_step_task_t *new_task,
//        smart_step_task_t *axis_x_task, smart_step_task_t *axis_y_task,
//        smart_step_task_t *axis_z_task, smart_step_task_t *axis_e_task);

#endif /* MOTION_CONTROL_H_ */
